#include "cmd_task.h"
#include "main.h"
#include "cmsis_os.h"
#include "stdlib.h"

#define ps2_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

static void control_init(control_t *control_init);
static void control_feedback_update(control_t *feedback_update);
static void control_loop(control_t *control_loop);
static void control_set(void);

static void fp32_UpdateMotorAngleSum(motor_t *moto);
static fp32 fp32_constrain(fp32 Value, fp32 minValue, fp32 maxValue);


control_t control;
extern uint16_t adc_val[2];

void cmd_task(void)
{
    vTaskDelay(500);
    
    control_init(&control);
    while(1)
    {
        control_feedback_update(&control);
        control_loop(&control);
        control_set();
        
        //can发送数据
        CAN_cmd_DMmotor4310_MIT(CAN_DM1_SLAVE_ID, 0, 0, 0, 0, control.motor1.given_current); 
        
        vTaskDelay(1);
    }
}

static void control_init(control_t *control_init)
{
    if(control_init == NULL)
    {
        return ;
    }
    
    //PID初始化
    static const fp32 dm1_speed_pid[3] = {DM_SPEED_PID_KP, DM_SPEED_PID_KI, DM_SPEED_PID_KD};
    static const fp32 dm1_position_pid[3] = {DM_POSITION_PID_KP, DM_POSITION_PID_KI, DM_POSITION_PID_KD};
    //初始化电机pid
    PID_init(&control_init->motor1.speed_pid, PID_POSITION, dm1_speed_pid, DM_SPEED_PID_MAX_OUT, DM_SPEED_PID_MAX_IOUT);
    PID_init(&control_init->motor1.position_pid, PID_POSITION, dm1_position_pid, DM_POSITION_PID_MAX_OUT, DM_POSITION_PID_MAX_IOUT);
    
    //电机数据指针获取
    control_init->motor1.motor_measure = get_dm_motor_1_measure_point();
    
    //使能电机
    for (uint8_t i = 0; i < 10; i++)
    {
        CAN_cmd_DMmotor_enable(CAN_DM1_SLAVE_ID);
    }
}

static void control_feedback_update(control_t *feedback_update)
{
    if (feedback_update == NULL)
    {
        return;
    }
    
    //更新ps2模块数据
    feedback_update->ps2.last_x = feedback_update->ps2.original_x;
    feedback_update->ps2.last_y = feedback_update->ps2.original_y;
    feedback_update->ps2.last_key = feedback_update->ps2.original_key;
    feedback_update->ps2.original_x = adc_val[0];
    feedback_update->ps2.original_y = adc_val[1];
    feedback_update->ps2.original_key = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_9);
    
    feedback_update->ps2.deflection_x = feedback_update->ps2.original_x - PS2_VRX_DEFLECTION;
    
    //死区限制，因为遥控器可能存在差异 摇杆在中间，其值不为0
    ps2_deadband_limit(feedback_update->ps2.deflection_x, feedback_update->ps2.remove_x, PS2_VRX_DEALINE);
    
    fp32_UpdateMotorAngleSum(&feedback_update->motor1);
}

static void control_loop(control_t *control_loop)
{
    if (control_loop == NULL)
    {
        return;
    }
    
    if(control.motor1.speed_mode == 0)
    {
        PID_calc(&control_loop->motor1.position_pid, control_loop->motor1.p_sum, control_loop->motor1.p_set);
        PID_calc(&control_loop->motor1.speed_pid, control_loop->motor1.motor_measure->v_fp32, control_loop->motor1.position_pid.out);
        control.motor1.given_current = control_loop->motor1.speed_pid.out;
    }
    else
    {
        PID_calc(&control_loop->motor1.speed_pid, control_loop->motor1.motor_measure->v_fp32, control_loop->motor1.v_set);
        control.motor1.given_current = control_loop->motor1.speed_pid.out;
    }
}

static void control_set(void)
{
    //按下ps2模块，电机角度增加90°
    if (control.ps2.original_key != 0) control.motor1.ps2key_flag = 1;
    if(control.ps2.original_key == 0 && control.motor1.ps2key_flag == 1 && control.motor1.ready_move_flag == 1)
    {
        control.motor1.ps2key_flag = 0;     //复位单次按键
        control.motor1.ready_move_flag = 0; //电机运行中
        control.motor1.speed_mode = 0;      //位置模式
        control.motor1.p_set = control.motor1.p_sum + PS2_SW_MOVE_ANGLE;
    }
    //移动到一定角度后，复位可移动标志位
    if(control.motor1.p_set - control.motor1.p_sum < 0.001f)
    {
        control.motor1.ready_move_flag = 1;
    }
    
    //ps2模块VRX通道控制电机正反转，且速度与拨动幅度成正比
    if(control.motor1.ps2key_flag == 1 && control.motor1.ready_move_flag == 1)
    {
        control.motor1.speed_mode = 1;      //速度模式
        control.motor1.v_set = PS2_VRX_SEN * control.ps2.remove_x;
        control.motor1.p_set = control.motor1.p_sum;    //防止按键时值突变
    }
}


static void fp32_UpdateMotorAngleSum(motor_t *moto)
{
    moto->last_p_calc = moto->p_calc;
    moto->p_calc = moto->motor_measure->p_fp32 + 12.50f; //6.28f;
    
    if (fabsf(moto->p_calc - moto->last_p_calc) > 12.50f)
    {
        if (moto->p_calc < moto->last_p_calc)
        {
            moto->p_sum += 25.00f - moto->last_p_calc + moto->p_calc;
        }
        else
        {
            moto->p_sum -= 25.00f - moto->p_calc + moto->last_p_calc;
        }
    }
    else
    {
        moto->p_sum += moto->p_calc - moto->last_p_calc;
    }
}

//限幅函数
static fp32 fp32_constrain(fp32 Value, fp32 minValue, fp32 maxValue)
{
    if (Value < minValue)
        return minValue;
    else if (Value > maxValue)
        return maxValue;
    else
        return Value;
}

